/**
 * 
 */
package com.zonerobotics.zBotAPI.ZBot;

import java.io.InputStream;
import java.io.OutputStream;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Date;
import java.util.HashMap;
import java.util.concurrent.ConcurrentHashMap;

import com.zonerobotics.zBotAPI.Enums.EzEvent;
import com.zonerobotics.zBotAPI.Enums.EzReadVars;
import com.zonerobotics.zBotAPI.Enums.EzWriteVars;
import com.zonerobotics.zBotAPI.Listeners.IBotListener;
import com.zonerobotics.zBotAPI.PollEngine.ZPacket;
import com.zonerobotics.zBotAPI.PollEngine.ZPollService;
import com.zonerobotics.zBotAPI.ZMsg.ZCameraInfo;
import com.zonerobotics.zBotAPI.ZMsg.ZHeader;
import com.zonerobotics.zBotAPI.ZMsg.ZImage;
import com.zonerobotics.zBotAPI.ZMsg.ZJoy;
import com.zonerobotics.zBotAPI.protocolMapping.VarAccessLevel;
import com.zonerobotics.zBotAPI.protocolMapping.VarMap;
import com.zonerobotics.zBotAPI.protocolMapping.VarMapping;



/**
 * @author Michael McCarty
 * todo how do we deal with images ? 
 * Feb 2, 2013
 */
public class ZBot implements IZBot{
	
	/**
	 * this holds all of the values you can modify in the bot, you must
	 * Access them through the setvalue and get value functions. There is
	 * a name and an integer value
	 */
	private static ConcurrentHashMap<String, Integer> botValues = new ConcurrentHashMap<String, Integer>();
	private VarMapping ourMapping;
	private boolean dirty = false;
	private String TAG = "ZBot";
	
	/**
	 * these will be default values for now, but may change given the versions
	 */
	private int writeStartIndex; 	// position, not bytes
	private int writeCount;			// gpiowrite and pwmduties
	private int txPkts = 0;			// amount of packets tx in state 2
	private int statModNum; 		// all packets are video, every X we get status
	
	public ZPollService pollEngine = null;


	/**
	 * this state increments as so we know what packet to get
	 */
	private int state = 0; 

	private ZCameraInfo camInfo;
	private int bpp; // bits per pixel (not important for jpeg)
	private static ZImage imageA;
	private static ZImage imageB;
	private boolean onImageA = true;
	private int imgSeqNum = 0;
	
	/**
	 * this is the last index of data we have got for the image
	 */
	private int lastImgIndex = 0;
	/**
	 * this is the len of data we read every time we get img data
	 */
	private int imgReadLen = 4352;
	/**
	 * frames per second, only valid if imageState is ready
	 */
	private long fps = 0;
	
	/*
	 * this is the listener/callback that we use to tell our clients they have
	 * a new image or new data available. 
	 */
	IBotListener ibl = null;
	/* (non-Javadoc)
	 * @see zonerobotics.com.zBotAPI.ZBot.IZBot#setIbl(zonerobotics.com.zBotAPI.ZBot.IBotListener)
	 */
	@Override
	public void setBotEventListener(IBotListener ibl) {
		this.ibl = ibl;
		this.pollEngine.setLogListener(ibl);
	}
	/**
	 * return frames per second of image, this should only be read
	 * while taking image or when imageState is ready
	 * @return
	 */
	public long getFps() {
		return fps;
	}

	/**
	 * returns the max buffer size we will request in our packets so the pollservice
	 * will know how big to make its buffers
	 * @return
	 */
	public int getMaxBufferSize()
	{
		return imgReadLen+512;
	}
	/***
	 * This gets the current stats and returns them 
	 * @return
	 */
	public ZBotStats getStats()
	{
		return pollEngine.zbs;
	}
	
/**
 * this constructor takes the streams and creates a new polling service
 * the bot maintains its polling service but it is public so the user
 * can access it also for example to stop 
 * @param stmInput
 * input stream to recieve data from
 * @param stmOutput
 * output stream to talk to the bot
 */
	public ZBot(InputStream stmInput, OutputStream stmOutput)
	{
		pollEngine = new ZPollService(stmInput, stmOutput, this);
		pollEngine.startSvc();
	}
	
    /* (non-Javadoc)
	 * @see zonerobotics.com.zBotAPI.ZBot.IZBot#botReady()
	 */
    @Override
	public boolean botReady()
    {
    	if(getState() >= 2)
    	{return true;}
    	return false;
    }
    
    public void raiseEvent(EzEvent event)
    {
    	if(ibl != null)
    	{
    		ibl.onNewEvent(event);
    	}
    	
    }
    
	/**
	 * This takes a varmapping for the bot, it is assumed
	 * that that already setup the variables for the version
	 * or bot
	 * @param vm
	 * varmapping where we will access the public mappingvars
	 */
	private void setMapping(int version)
	{
		VarMapping vm = new VarMapping(version);
		ourMapping = vm;
		// add all vars to the vars of this bot
		for(VarMap m: vm.mappingVars.values())
		{
			botValues.put(m.name, 0);
		}
	}
	
	/* (non-Javadoc)
	 * @see zonerobotics.com.zBotAPI.ZBot.IZBot#setValue(zonerobotics.com.zBotAPI.Enums.EzWriteVars, int)
	 */
	@Override
	public IZBot setValue(EzWriteVars name, Integer newValue)
	{
		String strName = name.toString();
		if(botValues.containsKey(strName))
		{
			if(botValues.get(strName) != newValue)
			{
				botValues.put(strName, newValue);
			}
			if(botValues.get(strName) != newValue)
			{
				sendLogMsg(TAG + 
						": Could Not update value, error: " + name.toString() + 
						" Current Value: " + botValues.get(strName));
			}else{
				this.dirty = true;	
			}
		}else{
			sendLogMsg(TAG + ": Could Not update value, key not found: " + strName);
		}
		return this;
	}
	
	/* (non-Javadoc)
	 * @see zonerobotics.com.zBotAPI.ZBot.IZBot#getValue(zonerobotics.com.zBotAPI.Enums.EzReadVars)
	 */
	@Override
	public Integer getValue(EzReadVars name)
	{
		return botValues.get(name.toString());
	}
	
	/**
	 * this returs the state if its anything less than
	 * two then this bot is not ready for service. 
	 * @return
	 */
	public int getState() {
		return state;
	}

	// todo movement, abstract ? will be overriden ! 
	
	/**
	 * uses the raw payload to update all the actual vars of the zbot 
	 * starting from the start position
	 * @param rawBytes
	 * the raw bytes payload portion of the packet
	 * @param StartPosition
	 * position number of the variable
	 * @return
	 */
	private IZBot updateVarsFromPayloadBytes(byte[] rawBytes, int StartPosition)
	{
		ByteBuffer bb = ByteBuffer.wrap(rawBytes);
		//bb.position(0x00);
		bb.order(ByteOrder.LITTLE_ENDIAN);
		String name = ourMapping.getNameFromPosition(StartPosition);
		int size = 0;
		
		// while we have bytes parse them out into our vars
		while(bb.hasRemaining())
		{
			name = ourMapping.getNameFromPosition(StartPosition);
			size = ourMapping.getSizeFromPosition(StartPosition);
			// if it is a "read var" read it in, else leave our write vars alone!
			if(ourMapping.getVarAccessLevel(name) != VarAccessLevel.WRITE_ONLY)
			{
				// one byte var
				if(size == 1)
				{
					this.botValues.put(name, (int) bb.get());
				}
				// two byte var
				if(size == 2)
				{
					this.botValues.put(name, (int) bb.getShort());
				}
				// four byte var
				if(size == 4)
				{
					this.botValues.put(name, (int) bb.getInt());
				}
			}else{
				for(int i=0; i<size; i++)
				{
					bb.get();
				}
			}
			
			StartPosition++;
		}
		return this;
	}
	
	/**
	 * this will always return the next packet we need to
	 * get, there is a state that keeps track of what we
	 * are doing so we know which packet to return
	 * @return
	 */
	public void getNextPacket(ZPacket nextPkt)
	{
		//ZPacket nextPkt = null;
		
		// if we are already setup for camera stuff
		if(camInfo != null)
		{
			// new image ready ! 
			if(lastImgIndex >= camInfo.getHeight()*camInfo.getWidth()*bpp && camInfo.getHeight() != 0)
			{
				// make sure we dont ask for lines past 
				// the lines available in our video
				lastImgIndex = 0;
				// calculate fps
				this.fps = System.currentTimeMillis() - this.fps;
				this.fps /= 1000; // TODO this now is really seconds per frame which is 6 ish
				// setup header of image
				ZHeader hdr = new ZHeader();
				hdr.setSeq(imgSeqNum++);
				hdr.setFrame_id("0");
				hdr.setTimestamp(new java.util.Date().getTime());
				if(this.onImageA)
				{
					imageA.setHeader(hdr);
				}else{
					imageB.setHeader(hdr);
				}
				if(ibl != null)
				{
					try{
						if(this.onImageA)
						{
							ibl.onNewEvent(EzEvent.imageA);
							onImageA = false; // start on image b
						}else{
							ibl.onNewEvent(EzEvent.imageB);
							onImageA = true; // go back to image a
						}
					}catch(Exception ex)
					{
						System.out.println("Failure in client app onNewEvent: " + ex.getMessage());
					}
				}
				//imgReadLen += 128;
				//System.out.println("new readlen , " + imgReadLen + " ," + this.pollEngine.zbs.getRXBps());
			}
			
			// first line of video start timers (start next image)
			if(lastImgIndex == 0 && camInfo.getHeight() != 0)
			{
				this.fps = System.currentTimeMillis();
			}
		}
		
		switch(state)
		{
			// we are brand new we need to get the ver
			// and model type
			case 0:
				nextPkt// = new ZPacket()
				.setWriteLength(0)
				.setWriteStartIndex(0)
				.setReadLength(4)
				.setReadStartIndex(0)
				.clearVideoFlag()
				.build();
			
			break;
			
			// we have the model/ver and our mapping
			// now we need to get all readonce vars
			case 1:
				nextPkt// = new ZPacket()
				.setWriteLength(0)
				.setWriteStartIndex(0)
				.setReadLength(ourMapping.totalSizeInBytes)
				.setReadStartIndex(0)
				.clearVideoFlag()
				.build();
			
			break;
			
			// we need to get status or image data
			case 2:

				// defaults (basic read stat msg)
				nextPkt// = new ZPacket()
				// most msg will be video
				.setVideoFlag()
				// no write by default
				.setWriteLength(0)
				.setWriteStartIndex(0);
				

				nextPkt
				.setReadLength(imgReadLen)
				.setReadStartIndex(lastImgIndex);
				
				int maxImgLen = (camInfo.getHeight()*camInfo.getWidth()*bpp);
				// we are requesting more than we need ! 
				if(lastImgIndex+imgReadLen > maxImgLen)
				{
					nextPkt.setReadLength(maxImgLen-lastImgIndex);
				}

				
				// DEBUG ONLY TODO REMOVE
				//this.sendLogMsg("VidLine: " + lastVideoLine + " Offset: "+ lastVideoLine*videoWidth*Bpp + " ReadLen: " + videoWidth*Bpp);
				
				// if this is dirty we need to add the 
				// new values of the bot to the packet so 
				// they are written
				if(this.dirty)
				{
					// make bot to bytes, from start index count in vars
					nextPkt.setVarPayload(this.toBytePayload(writeStartIndex, writeCount));
					// we will be writing this amount
					nextPkt.setWriteLength(nextPkt.getVarPayloadLen());
					// the offset in bytes based off the position
					nextPkt.setWriteStartIndex(ourMapping.getByteOffsetFromPosition(writeStartIndex));
					// TODO verify next step
					// assume this packet is sent
					this.dirty = false;
				}
				
				// if we should be getting a status message 
				// also if the frame is ready and has not been read, dont overwrite it
				if((this.txPkts % statModNum) == 0)
				{
					// stop vid msg
					nextPkt.clearVideoFlag()
					// set basics for read of stat msg
					//.setReadLength(readBytes)
					//.setReadStartIndex(ourMapping.getByteOffsetFromPosition(readStartIndex));
					// search EVERYWHERE ! 
					.setReadLength(ourMapping.totalSizeInBytes)
					.setReadStartIndex(0);
				}
				// TODO 
				
				nextPkt.build();
				
				this.txPkts++;
			break;
			
			default:
				break;
		
		}
	}

	/**
	 * this returns true if we have a valid version on
	 * this bot, right now the only version is 1
	 * @param botVer
	 * @return
	 */
	public boolean validVersion(int botVer)
	{
		if(botVer > 0 && botVer < 2)
		{
			return true;
		}
		return false;
		
	}

	private boolean setupBot(int version, int model)
	{

		
		if(!validVersion(version))
		{
			sendLogMsg(TAG + ": Invalid Bot Version: " + version);
			return false; // bad version
		}
		
		// setup our mapping
		this.setMapping(version);	
		
		if(version == 1)
		{
			writeStartIndex = 14; 	// position, not bytes
			writeCount = 9;			// gpiowrite and pwmduties
			statModNum = 10; 	// all packets are video, every X we get status
			camInfo = new ZCameraInfo();
		}
		
		return true;
	}

	/**
	 * this will update the bot given a new packet, 
	 * it will change the state depending on the packet sent 
	 * etc
	 * @param pkt
	 * new incoming packet
	 * @return
	 * true if able to update
	 */
	public boolean updateFromPacket(ZPacket pkt)
	{
		switch(state)
		{
			// we are new we need the model number and ver from the 
			// never changing first two vars
			case 0:
				// must be 2 int 16, or 4 bytes
				if(pkt.getVarPayload() == null || pkt.getVarPayloadLen() != 4)
				{
					sendLogMsg(TAG + ": bad length in state: " + state);
					return false;
				}
				if(pkt.hasVideoData())
				{
					sendLogMsg(TAG + ": got video while expecting status: " + state);
					return false;
				}
	    		// parse out the model and version
	    		ByteBuffer bb = ByteBuffer.wrap(pkt.getVarPayload()).order(ByteOrder.LITTLE_ENDIAN);
	    		int botModel = bb.getShort();
	    		int botVersion = bb.getShort();
	    		// setup all internals now that we know the version
	    		if(!this.setupBot(botVersion, botModel))
	    		{
	    			return false;
	    		}
	    		// change state
	    		this.state = 1; // state 1 is got version and set mapping
				if(ibl != null)
				{
					ibl.onNewEvent(EzEvent.connected);
				}
	    		return true;
	    		
	    	// this should contain all possible vars even read once, this is
	    	// our initial setup 
			case 1:
				// if its not there or not big enough it can't be right
				if(pkt.getVarPayload() == null || pkt.getVarPayloadLen() < ourMapping.totalSizeInBytes)
				{
					sendLogMsg(TAG + ": bad length in state: " + state);
					return false;
				}
				if(pkt.hasVideoData())
				{
					sendLogMsg(TAG + ": got video while expecting status: " + state);
					return false;
				}
				// update our bot, start from the first var
				this.updateVarsFromPayloadBytes(pkt.getVarPayload(), 0);
				// how big is our image buffer
				this.camInfo.setWidth(this.getValue(EzReadVars.VideoWidth));
				this.camInfo.setHeight(this.getValue(EzReadVars.VideoHeight));
				bpp = this.getValue(EzReadVars.VideoBPP);
				
				// setup our images
				if(imageA == null)
				{
					imageA = new ZImage();
					imageA.setWidth(camInfo.getWidth());
					imageA.setHeight(camInfo.getHeight());
					imageA.setStep(camInfo.getWidth() * bpp);
					imageA.setEncoding("RGB_565");
					imageA.setData(new byte[imageA.getStep()*imageA.getHeight()]);
				}
				if(imageB == null)
				{
					imageB = new ZImage();
					imageB.setWidth(camInfo.getWidth());
					imageB.setHeight(camInfo.getHeight());
					imageB.setStep(camInfo.getWidth() * bpp);
					imageB.setEncoding("RGB_565");
					imageB.setData(new byte[imageB.getStep()*imageB.getHeight()]);
				}

				// move onto next state, we are ready to poll
				this.state = 2; 
				return true;
				
			case 2:
				// if its not there or not big enough it can't be right
				if(pkt.getVarPayload() == null)
				{
					sendLogMsg(TAG + ": null pkt: " + state);
					return false;	
				}
				/* this was for testing
				this.setValue(EzWriteVars.gpioWrite, 0xFF);
				this.setValue(EzWriteVars.PWMOUTCH1Duty, 10);
				this.setValue(EzWriteVars.PWMOUTCH2Duty, 20);
				this.setValue(EzWriteVars.PWMOUTCH3Duty, 30);
				this.setValue(EzWriteVars.PWMOUTCH4Duty, 40);
				this.setValue(EzWriteVars.PWMOUTCH5Duty, 50);
				this.setValue(EzWriteVars.PWMOUTCH6Duty, 60);
				this.setValue(EzWriteVars.PWMOUTCH7Duty, 70);
				this.setValue(EzWriteVars.PWMOUTCH8Duty, 80);
				*/
				// if its video 
				if(pkt.hasVideoData())
				{
					// i think this is already checked in txrx
//					if(pkt.getVarPayloadLen() != imgReadLen)
//					{
//						sendLogMsg(TAG + ": bad Video Data Len: " + pkt.getVarPayloadLen());
//						return false; 
//					}
					try{
						// copy it into the right line in rawFrame
						if(this.onImageA)
						{
							System.arraycopy(pkt.getVarPayload(), 0, imageA.getData(), lastImgIndex, pkt.getVarPayloadLen());
						}else{
							System.arraycopy(pkt.getVarPayload(), 0, imageB.getData(), lastImgIndex, pkt.getVarPayloadLen());
						}
					}catch (Exception ex)
					{
						sendLogMsg(TAG + ": Could not write data to img buffer Line: " + lastImgIndex + " Len: " + pkt.getVarPayloadLen() + " err: " + ex.getMessage());
					}
					
					lastImgIndex += imgReadLen; // incrament line count

				}
				else // else it is a status packet
				{
					if(pkt.getVarPayloadLen() != ourMapping.totalSizeInBytes)//readBytes)
					{
						sendLogMsg(TAG + ": bad status Data Len: " + pkt.getVarPayloadLen());
						return false; 
					}
					// update our bot, start from the first var
					//this.updateVarsFromPayloadBytes(pkt.getVarPayload(), readStartIndex);
					this.updateVarsFromPayloadBytes(pkt.getVarPayload(), 0);
					// move onto next state, we are ready to poll
					this.state = 2; 
				}
				return true;

				
		default:
				break;
		
		}	
		
		return false;
	}
	public ZImage getImageA() {
		return imageA;
	}
	public ZImage getImageB() {
		return imageB;
	}
	private void sendLogMsg(String msg)
    {
    	 //Message m = mHandler.obtainMessage(ControlbotActivity.MESSAGE_MSG);
    	 //m.obj = msg;
    	 //m.sendToTarget();
		System.out.println(msg);
    }
	
	private byte[] toBytePayload(int StartPosition, int count)
	{
		ByteBuffer payload = null;
		int size = 0;
		int bytesSize = 0;
		int endPos = (StartPosition+count);
		String name;
		int oldValue;
		
		for(int i = StartPosition; i<endPos; i++)
		{
			size += ourMapping.getSizeFromPosition(i);
		}

		payload = ByteBuffer.allocate(size);
		payload.order(ByteOrder.LITTLE_ENDIAN);
		for(int i = StartPosition; i<endPos; i++)
		{
			name = ourMapping.getNameFromPosition(i);
			oldValue = botValues.get(name);
			bytesSize = ourMapping.getSizeFromPosition(i);
			if(bytesSize == 1)
			{
				payload.put((byte)( oldValue & 0xFF ));
			}
			// two byte var
			if(bytesSize == 2)
			{
				payload.putShort((short)( oldValue & 0xFFFF ));
			}
			// four byte var
			if(bytesSize == 4)
			{
				 payload.putInt(oldValue);
			}	
		}
			
		return payload.array();
	}
	
	/**
	 * This will kill the polling service and its threads. After this the bot is 
	 * nothing go ahead and remove it you will need to start another.
	 */
	@Override
	public void killBot()
	{
		this.pollEngine.stopSvc();
		this.pollEngine.killSvc();
	}
	
	@Override
	public void onJoyChange(ZJoy joy) {

		// TODO Auto-generated method stub
		if(joy.getButtons().length < 10 || joy.getAxes().length < 3)
		{
			// mal formed joy object, not enough buttons or axes
			sendLogMsg(TAG + ": bad Joy Message: ");
		}
		int newGPIO = 0;
		for(int i=0; i< joy.getButtons().length; i++)
		{
			if(joy.getButtons()[i] == 1)
			{   // set bit
				newGPIO |= (1 << i);
			}else{
				// clear bit
				newGPIO &= (0xFF | (1<<i));
			}
			
		}
		this.setValue(EzWriteVars.gpioWrite, newGPIO);
		// TODO buffer messages on other thread ? 
		float[] jAxes = joy.getAxes();
		if(jAxes.length >= 2)
		{
			this.setValue(EzWriteVars.PWMOUTCH1Duty, (int) (jAxes[0]*255));
			this.setValue(EzWriteVars.PWMOUTCH2Duty, (int) ((int) -1*(jAxes[0]*255)));
			this.setValue(EzWriteVars.PWMOUTCH3Duty, (int) (jAxes[1]*255));
			this.setValue(EzWriteVars.PWMOUTCH4Duty, (int) ((int) -1*(jAxes[1]*255)));

		}
		// if they have other motors hooked up
		if(jAxes.length >= 3)
		{
			this.setValue(EzWriteVars.PWMOUTCH5Duty, (int) (jAxes[2]*255));
			this.setValue(EzWriteVars.PWMOUTCH6Duty, (int) (jAxes[2]*255));
		}
	}


	
}
